#include <iostream>
#include <time.h>
#include <arpa/inet.h>
#include <termios.h>
#include <fcntl.h>
#include <cstdio>
#include <cstdio>
#include <cerrno>
#include <cstring>
#include <cstdlib>
#include <getopt.h>

#include "dataDevice.h"
#include "signal.h"

using namespace std;

const char ConfigFile[]= "config";

/*--------------------ethernetDataDevice-----------------*/
ethernetDataDevice::ethernetDataDevice(char const * addr, int port)
{
	this->addr = addr;
	this->port = port;
}

bool ethernetDataDevice::open()
{
	Signal(SIGPIPE,SIG_IGN);  /* avoid write a broken tcp connect lead to program exit */

	if((fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
	{
		cerr<<"create ethernet data device socket file describe error: "<<strerror(errno)<<endl;
		return false;
	}
	
	struct sockaddr_in dataDeviceAddr;
	dataDeviceAddr.sin_family = AF_INET;
	dataDeviceAddr.sin_port = htons(port);
	inet_pton(AF_INET, addr, &dataDeviceAddr.sin_addr);

	if(connect(fd, (struct sockaddr*)&dataDeviceAddr, sizeof(struct sockaddr)) == -1)
	{
		cerr<<"connect to "<<addr<<" : "<<port<<" error "<<strerror(errno)<<endl;
		return false;
	}
	
	cout<<"open ethernet device"<<endl;
	return true;
}

ssize_t ethernetDataDevice::read(void *buffer, size_t size)
{
	cout<<"Device: Start reading"<<endl;
	int res = recv(fd, buffer, size, 0);
	cout<<"Device: read "<<res<<" bytes from device"<<endl;
	if(res == -1)
	{
    cout<<"reconnecting....."<<endl;
		close(fd);
		open();
		cout<<"Please send cmd agine"<<endl;
	}
	return res;
}

ssize_t ethernetDataDevice::write(const void *buffer, size_t size)
{
  //char *data = static_cast<char *>(buffer);
	cout<<"Device write"<<endl;
	int res = send(fd, buffer, size, 0);
	cout<<"write "<<res<<" bytes to device"<<endl;
	if(res == -1)
	{
		cout<<"reconnecting....."<<endl;
		close(fd);
		open();
		res = write(buffer, size);
	}
	return res;
}

ethernetDataDevice::~ethernetDataDevice()
{
	close(fd);
}
/*-------------------serialDataDevice--------------------*/

bool setSpeed(int fd, int speed)
{
	int status;
	struct termios opt;

	if(tcgetattr(fd, &opt) == -1)
	{
		cerr<<"tcgetattr error: "<<strerror(errno)<<endl;
		return false;
	}
	/*
	if(tcflush(fd, TCIOFLUSH) == -1)
	{
		cerr<<"tcflush error: "<<strerror(errno)<<endl;
		return false;
	}
	*/
	if(cfsetispeed(&opt, speed) == -1)
	{
		cerr<<"cfsetispeed error: "<<strerror(errno)<<endl;
		return false;
	}
	if(cfsetospeed(&opt, speed) == -1)
	{
		cerr<<"cfsetospeed error: "<<strerror(errno)<<endl;
		return false;
	}

	if(tcsetattr(fd, TCSANOW, &opt) == -1)
	{
		cerr<<"tcsetattr error: "<<strerror(errno)<<endl;
		return false;
	}
	if(tcflush(fd, TCIOFLUSH) == -1)
	{
		cerr<<"tcflush error: "<<strerror(errno)<<endl;
		return false;
	}

	return true;
}

bool set_data_format(int fd, int databits, int parity, int stopbits)
{
	//fcntl(fd, F_SETFL, 0);
	struct termios opt;

	if(tcgetattr(fd, &opt) == -1)
	{
		cerr<<"tcgetattr error: "<<strerror(errno)<<endl;
		return false;
	}

	opt.c_cflag &= ~CSIZE;

	switch(databits)
	{
		case 5:
		  opt.c_cflag |= CS5;
			break;
		case 6:
		  opt.c_cflag |= CS6;
			break;
		case 7:
		  opt.c_cflag |= CS7;
			break;
		case 8:
		  opt.c_cflag |= CS8;
			break;
		default:
		  cerr<<"Unsupported data size"<<endl;
			return false;
	}

	switch(parity)
	{
		case 'n':
		case 'N':
		  opt.c_cflag &= ~PARENB;
			opt.c_iflag &= ~INPCK;
			break;
		case 'o':
		case 'O':
		  opt.c_cflag |= (PARODD|PARENB);
			opt.c_iflag |= INPCK;
			break;
		case 'e':
		case 'E':
		  opt.c_cflag |= PARENB;
			opt.c_cflag &= ~PARODD;
			opt.c_iflag |= INPCK;
			break;
		defalut:
		  cerr<<"Unsupported parity"<<endl;
			return false;
	}

	switch(stopbits)
	{
		case 1:
		  opt.c_cflag &= ~CSTOPB;
			break;
		case 2:
		  opt.c_cflag |= ~CSTOPB;
			break;
		default:
		  cerr<<"Unsupported stop bits"<<endl;
			return false;
	}

	/* Set input parity option */
	if(parity != 'n' && parity != 'N')
	  opt.c_iflag |= INPCK;

	opt.c_cflag |= (CLOCAL | CREAD);
  /*
	opt.c_cc[VTIME] = 1; //0.1 seconds
	opt.c_cc[VMIN] = 10;
	*/

	//opt.c_lflag |= (ICANON | ECHO | ECHOE);
	opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  
	if(tcsetattr(fd, TCSANOW, &opt) == -1)
	{
		cerr<<"tcsetattr error: "<<strerror(errno)<<endl;
		return false;
	}

	if(tcflush(fd, TCIFLUSH) == -1)	/* Update the options and do it Now*/
	{
	  cerr<<"tcflush error: "<<strerror(errno)<<endl;
		return false;
	}

  fcntl(fd, F_SETFL, 0);
	return true;
}

serialDataDevice::serialDataDevice(char const * serialName)
{
	this->serialName = serialName;
}

bool serialDataDevice::open()
{
	fd = ::open("/dev/ttyTX1", O_RDWR | O_NOCTTY);

	struct termios opt;
	tcgetattr(fd, &opt);

  cfsetispeed(&opt, B57600);
  cfsetospeed(&opt, B57600);

  opt.c_lflag &= ~(ECHO | ICANON | IEXTEN | ISIG);
  opt.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
  opt.c_oflag &= ~(OPOST);
  opt.c_cflag &= ~(CSIZE | PARENB);
  opt.c_cflag |= CS8;

	tcsetattr(fd, TCSANOW, &opt);

	/*
	if((fd = ::open(serialName, O_RDWR | O_NOCTTY | O_NDELAY)) == -1)
	{
		cerr<<"Open serial error: "<<strerror(errno)<<endl;
		return false;
	}
	if(!setSpeed(fd, B57600))
	  return false;
	if(!set_data_format(fd, 8, 'N', 1))
	  return false;
	*/
  return true;
}

ssize_t serialDataDevice::read(void *buffer, size_t size)
{
  return ::read(fd, buffer, size);
}

ssize_t serialDataDevice::readn(void *buffer, size_t size)
{
  int nleft = size;
  int nread;
  char *ptr = (char *)buffer;
  while(nleft>0)
  {
    nread = ::read(fd, buffer, nleft);
	if(nread<0)
	{
	  if(errno == EINTR)
	    nread = 0;
      else
	    return -1;
	}
	else if(nread == 0)
	  break;
    nleft -= nread;
    ptr += nread;
  }

  return size-nleft;
}

ssize_t serialDataDevice::write(const void *buffer, size_t size)
{
	int res = ::write(fd, buffer, size);
	cout<<"write "<<res<<" bytes to device"<<endl;
	return res;
}

serialDataDevice::~serialDataDevice()
{
	cout<<"Close serial device"<<endl;
	::close(fd);
}

unsigned short int GetCrc16(char* InStr, unsigned int len)
{
    unsigned short int crc = 0;
    //cout<<"sizeof crc "<<sizeof(crc)<<endl;
    for(int i = 0; i < len; i++)
    {
        crc = crc ^ InStr[i] << 8;
        for(int j = 0; j < 8; j++)
        {
            if((crc & 0x8000) != 0)
                crc = (((crc << 1) ^ 0x1021)) & 0xffff;
            else
                crc = (crc<<1) & 0xffff;
        }
        //cout<<crc<<endl;
    }

    //cout<<crc<<endl;
    return crc & 0xffff;
}

int litend(void)
{
  int i = 0;
  ((char *)(&i))[0] = 1;
  return (i == 1);
}

int pack(unsigned char *buff, int size, char *sbuff)
{
  char sendBuff[100];
	int len = 0;

	//Add header
	memcpy(sendBuff, "\x44\x00\x00\xFF\xFF\x00\x00", 7);
	len += 7;

	//Add length
	sendBuff[len] = char(size);
	len += 1;

	//Add ???
	memcpy(sendBuff + len, "\x00\x89", 2);
	len += 2;

	//Add payload
	memcpy(sendBuff + len, buff, size);
	len += size;

	//caculate crc16
	unsigned short crc;
	crc = GetCrc16(sendBuff, len);

	//Add crc
	if(litend()) //little endian
	{
		sendBuff[len ] = ((char *)(&crc))[0];
		sendBuff[len + 1] = ((char *)(&crc))[1];
	}
	else        //big endian
	{
		sendBuff[len] = ((char *)(&crc))[1];
		sendBuff[len + 1] = ((char *)(&crc))[0];
	}
	len += 2;

	int c = 1;

	//Add start flag
	sbuff[0] = 0x7e;

	for(int i=0;i<len;i++)
	{
		if(sendBuff[i] == 0x7d)
		{
			sbuff[i + c] = 0x7d;
			sbuff[i + c + 1] = 0x5d;
			c++;
		}
		else if(sendBuff[i] == 0x7e)
		{
			sbuff[i + c] = 0x7d;
			sbuff[i + c + 1] = 0x5e;
			c++;
		}
		else
			sbuff[i + c] = sendBuff[i];
	}

	//Add end
	sbuff[len + c] = 0x7e;

	return len + c + 1;
}

int main(int argc, char **argv)
{
	serialDataDevice sdd("/dev/ttyTX1");
	sdd.open();
	//char on[]   = {0x01, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
	//char off[]  = {0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
	//char data[] = {0x02, 0x02, 0x0a, 0x02, 0x00, 0x00, 0x00, 0x34, 0x00, 0x55, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
	
	unsigned char data[22];
	bzero(data, sizeof(data));
	
	int c;
	int usleepTime = 0;
	int count = 0;
	bool sendFlag = true;
	bool recvFlag = false;
	int errFlag = 0;
	
	struct option longopts[] = 
	{
		{"receive", 0, 0, 'r'},
		{"destination", required_argument, 0, 'd'},
		{"usleep", required_argument, 0, 'u'},
		{"count", required_argument, 0, 'c'},
		{"type", required_argument, 0, 't'},
		{0, 0, 0, 0}
	};
  
	while((c = getopt_long(argc, argv, "rd:u:c:t:", longopts, NULL)) != EOF)
	{
		switch(c)
		{
			case 'd':
				data[1] = (char)atoi(optarg);
				cout<<"Destination node "<<atoi(optarg)<<endl;
				break;
			case 'u':
				usleepTime = atoi(optarg);
				cout<<"Sleep "
				    <<atoi(optarg)
						<<" microseconds before write data to serial."
						<<endl;
				break;
			case 't':
				if(0 == strcmp(optarg, "on"))
				{
					data[0] = '\x01';
					sendFlag = false;
					cout<<"Turn on 433M radio"<<endl;
				}
				else if(0 == strcmp(optarg, "off"))
				{
					data[0] = '\x00';
					sendFlag = false;
					cout<<"Turn off 433M radio"<<endl;
				}
				else
					errFlag++;
				break;
			case 'c':
				count = atoi(optarg);
				cout<<"Will send "<<count<<" packages."<<endl;
				break;
			case 'r':
				recvFlag = true;
				break;
			case '?':
				errFlag++;
				cout<<"2"<<endl;
				break;
			default:
				break;
		}
	}

	if(errFlag)
	{
		cerr<<"Usage:	dataDevice [OPTION]"<<endl<<endl
		    <<"\t-r, --receive	Receive data"<<endl
		    <<"\t-d, --destination	destination node number"<<endl
		    <<"\t-u, --usleep	microseconds sleep before send data"<<endl
		    <<"\t-c, --count	number of package to send"<<endl
		    <<"\t-t, --type	on or off"<<endl;

		exit(0);
	}
	
	if(sendFlag)
		data[0] = '\x02';
	else
		count = 1;

	char buff[200];
	char rbuff[200];

	struct timeval start, end;
	struct timespec ts = {0, usleepTime*1000};
	double avg = 0.0;

	int num = count;;
	int res;
	
	int dataIndex = 2;
	while(!recvFlag && count--)
	{
		data[dataIndex]++;
		if(data[dataIndex] == '\x00')
			dataIndex++;
		if(dataIndex == 22)
			dataIndex = 2;
			
		res = pack(data, sizeof(data), buff);
		if(usleepTime)
			nanosleep(&ts, NULL);
		gettimeofday(&start, NULL);
		sdd.write(buff, res);
		for(int i =0;i< res;++i)
			printf("%02hhx ", buff[i]);
		cout<<endl;
		
		int n = 0;
		while(true)
		{
			res = sdd.read(rbuff, sizeof(rbuff));
			cout<<res<<endl;
			
			for(int i = 0;i< res;i++)
				printf("%02hhx ", rbuff[i]);
			cout<<endl;
			
			n+=res;
			if(n==41)
			{
				gettimeofday(&end, NULL);
				avg +=(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec)/1000000.0;
				cout<<"used "
					<<(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec)/1000000.0
					<<" seconds from write serial to receive two ACK"<<endl;

				break;
			}
		}
	}
	
	if(!recvFlag)
		cout<<"Avg: "<<avg/num<<endl;
		
	while(recvFlag)
	{
		res = sdd.read(rbuff, sizeof(rbuff));
		cout<<res<<endl;

		for(int i = 0;i< res;i++)
			printf("%02hhx ", rbuff[i]);
		cout<<endl;
	}

	return 0;
}
